﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using KSASupport.BlobTracking;

namespace KSASupport
{
    /// <summary>
    /// Class to handle all image analysis/detection for blobs.
    /// Handles image thresholding as well.  Input should be a depth map, and output
    /// will be a series of blob objects, with x,y coordinates (relative to input image), bounding boxes, and heights
    /// above the screen
    /// </summary>
    public class BlobDetector
    {

        #region Filter Settings
        private int minimumBlobSPixelCount;
        /// <summary>
        /// Filter option: Removes blobs which contain less pixels than this setting
        /// </summary>
        public int MinimumBlobSPixelCount
        {
            get { return minimumBlobSPixelCount; }
            set { minimumBlobSPixelCount = value; }
        }

        private int maximumBlobPixelCount;
        /// <summary>
        /// Filter Option: Filters blobs which contain more pixels than this setting
        /// </summary>
        public int MaximumBlobPixelCount
        {
            get { return maximumBlobPixelCount; }
            set { maximumBlobPixelCount = value; }
        }

        private int lastImagePixelCount;

        /// <summary>
        /// Tracks the number of pixels detected in the last frame. Could be used to find 'invalid' frames,
        /// or if a large amount of blobs/data is being passed up. Calculated at the Threshold level.
        /// </summary>
        public int LastImagePixelCount
        {
            get { return lastImagePixelCount; }
            set { lastImagePixelCount = value; }
        }

        private int minimumBlobPixelHeight;
        /// <summary>
        /// Filter Setting: Filters out blobs which, sizewise, are not higher than this setting
        /// </summary>
        public int MinimumBlobPixelHeight
        {
            get { return minimumBlobPixelHeight; }
            set { minimumBlobPixelHeight = value; }
        }

        private int minimumBlobPixelWidth;

        /// <summary>
        /// Filter setting: Filters out blobs which are not wider than this setting
        /// </summary>
        public int MinimumBlobPixelWidth
        {
            get { return minimumBlobPixelWidth; }
            set { minimumBlobPixelWidth = value; }
        }

        /// <summary>
        /// Trackes the average Frames to use for the Weighted Moving Average technique
        /// </summary>
        private int averageFrameCount;

        public int AverageFrameCount
        {
            get { return averageFrameCount; }
            set { averageFrameCount = value; }
        }

        #endregion

        private int[] ThresholdImage;
        private int DepthWidth;
        private int DepthHeight;
        private Rect boundaryFilterRect;

        public BlobDetector(int depthWidth, int depthHeight,Rect BoundaryFilterRect) 
        {
            this.DepthHeight = depthHeight;
            this.DepthWidth = depthWidth;
            ThresholdImage = new int[depthHeight * DepthWidth];
            // note: round/cast the coordinates here, as we'll use them during our scans to detect blobs which run along the edge
            boundaryFilterRect = new Rect(new Point((int)BoundaryFilterRect.TopLeft.X,(int)BoundaryFilterRect.TopLeft.Y),new Point((int)BoundaryFilterRect.BottomRight.X,(int)BoundaryFilterRect.BottomRight.Y));

            // init filters to 'off' (-1)
            minimumBlobPixelHeight = -1;
            minimumBlobPixelWidth = -1;
            maximumBlobPixelCount = -1;

            // Number of frames to average across
            averageFrameCount = 3;
        }

        /// <summary>
        /// Used to convert a point in a byte array to an X/Y point
        /// </summary>
        /// <param name="index"></param>
        /// <param name="stride">The width (pixels * bytes per pixel) of the image</param>
        /// <returns></returns>
        public Point GetPointFromIndex(int index, int stride)
        { 
            Point p = new Point();
            p.Y = (int)(index / stride);
            p.X = index % stride;
            return p;
        }

        public List<DepthBlob> GetBlobsFromDepthImage(int[] depthImage, string KinectID, string Source, bool LabelEdge)
        {
            const string DepthConversion2D = "2D Depth Conversion";
            // informational purposes for partitioning..finds number of 'logical' processors available
            int procs = Environment.ProcessorCount;

            List<DepthBlob> blobs = new List<DepthBlob>();
            string PerfMetric = "GetBlobsFromImage (" + KinectID + ")" + " Source: " + Source;

            Performance.PerformanceManager.StartMeasuring(DepthConversion2D);
            int[,] DepthImage2D = ConvertDepthImageTo2D(depthImage);
            Performance.PerformanceManager.StopMeasuring(DepthConversion2D);
            // Logic is as follows:
            // Split the screen vertically. Find blobs in each region (using procs to parallelize this)
            // At the end, put blobs back together
            Performance.PerformanceManager.StartMeasuring(PerfMetric);            
            blobs = GetDepthBlobsFromSection(DepthImage2D,0, 320,LabelEdge);
            Performance.PerformanceManager.AddData(PerfMetric, " Detected blobs: " + blobs.Count);
            Performance.PerformanceManager.StopMeasuring(PerfMetric);
            return blobs;
        }

        /// <summary>
        /// Algo description:
        /// 1) Find first and last contiguous point
        /// 2) For each blob found, find if
        ///   A) First point is between first and last point of any blob
        ///   B) Last point is between first and last point of any blob
        ///   C) First point is less AND last point is greater than both points of any blob
        /// 3) For ALL matches, JOIN blobs
        /// Continue processing from last contiguous point
        /// </summary>
        /// <param name="depthImage"></param>
        /// <param name="start"></param>
        /// <param name="verticalPartition"></param>
        /// <returns></returns>
        public List<DepthBlob> GetDepthBlobsFromSection(int[,] depthImage, int start, int verticalPartition, bool LabelSides)
        {
            List<DepthBlob> depthBlobs = new List<DepthBlob>();
            List<SimpleBlob> blobs = new List<SimpleBlob>();

            for (int y = 0; y < DepthHeight; y++)
            {
                for (int x = start; x < verticalPartition; x++)
                {               
                    if (depthImage[x, y] > 0)
                    {
                        // Found the start pixel! Find the 'last' pixel
                        Point startPoint = new Point(x, y);
                        Point endPoint = new Point(x, y);
                        while((x < verticalPartition) && (depthImage[x,y] > 0))
                        {
                            x++;
                        }
                        // Current value of X-1 will be 'last' pixel
                        endPoint = new Point(x - 1, y);

                        // At this point we have a start and end point. Look through all current
                        // blobs to see if either Start or End are between previous start/end points
                        // then look to see if start AND end are outside of start/end points.
                        // If that is true, then join this blob and the other and continue. There
                        // may be multiple matches
                        bool JoinedBlob = false; // track if this blobs been 'joined' yet.
                        
                        // if 'new' then create a new blob. if not new, then just update an old blob
                        // if 'joined' then mark any after that 'invalid' so we ignore them.
                        // this shouldn't happen often (U or n case shapes I believe)
                        SimpleBlob newBlob = null;
                        foreach (SimpleBlob blob in blobs)
                        {
                            if (IsSameBlob(startPoint, endPoint, blob.startPoint, blob.endPoint))
                            {
                                if (!JoinedBlob)
                                {
                                    // Update blob with these points
                                    blob.points.Add(startPoint);
                                    blob.points.Add(endPoint);
                                    blob.startPoint = startPoint;
                                    blob.endPoint = endPoint;                                    
                                    newBlob = blob;
                                    JoinedBlob = true;
                                }
                                else
                                {
                                    
                                    // We found a 'second' match. Mark it as invalid, and assimilate
                                    // its points
                                    newBlob.points.AddRange(blob.points);                                    
                                    blob.IsValid = false;
                                }
                            }
                        } // end analyzing blobs
                        
                        
                        // if newBlob is still null, we didnt find a match. Create a new blob
                        if (newBlob == null)
                        { 
                            newBlob = new SimpleBlob();
                            newBlob.points.Add(startPoint);
                            newBlob.points.Add(endPoint);
                            newBlob.startPoint = startPoint;
                            newBlob.endPoint = endPoint;
                            newBlob.Id = blobs.Count + 1;
                            blobs.Add(newBlob);
                        }
                    } // end 'found point' loop
                } // end for loop iterating through X axis
            } // end for loop iterating through y axis

            foreach (SimpleBlob blob in blobs)
            {
                if (blob.IsValid)
                {
                    DepthBlob depthBlob = new DepthBlob();
                    //depthBlob.Id = blob.Id;
                    depthBlob.Points.AddRange(blob.points);
                    depthBlob.Update();
                    // TODO: Parametize this portion for blob filtering
                    if (((depthBlob.BoundingBox.Width >= minimumBlobPixelWidth) || 
                        (depthBlob.BoundingBox.Height >= minimumBlobPixelHeight)) && 
                        (depthBlob.Points.Count() >= minimumBlobSPixelCount))
                    {
                        double i = depthBlob.BoundingBox.Width;
                        double j = depthBlob.BoundingBox.Height;
                        depthBlob.Side = blob.side;
                        depthBlob.EdgePoint = blob.edgePoint;
                        depthBlobs.Add(depthBlob);
                    }                  
                }
            }
            return depthBlobs;
        }

        /// <summary>
        /// If First point is between startAbove/endAbove, OR
        /// if last point is between, OR
        /// if both are outside..then TRUE
        /// </summary>
        /// <param name="startCurrent"></param>
        /// <param name="endCurrent"></param>
        /// <param name="startAbove"></param>
        /// <param name="endAbove"></param>
        /// <returns></returns>
        private bool IsSameBlob(Point startCurrent,Point endCurrent, Point startAbove, Point endAbove)
        {
            // if the blob is not directly above, then these points are not part of it.
            if ((startCurrent.Y - 1) != startAbove.Y)
                return false;

            if ((startCurrent.X >= startAbove.X) && (startCurrent.X <= endAbove.X))
                return true;

            if ((endCurrent.X >= startAbove.X) && (endCurrent.X <= endAbove.X))
                return true;

            if ((startCurrent.X <= startAbove.X) && (endCurrent.X >= endAbove.X))
                return true;

            return false;
        }
        /// <summary>
        /// For some blob detection algos, we'll want to deal with this in a 2D array.
        /// Note: This can take over a millisecond to process, so there is an opportunity here
        /// to optimize, either via parallelizing the process using Parallel.For, or by
        /// handing the arraay in from the DepthManager as a 2D array to start. Coding4FunToolkit
        /// would be useful here.
        /// </summary>
        /// <param name="depthImage"></param>
        /// <returns></returns>
        private int[,] ConvertDepthImageTo2D(int[] depthImage)
        {
            int[,] DepthImage2D = new int[DepthWidth,DepthHeight];
            for (int y = 0; y < DepthHeight; y++)
            {
                for (int x = 0; x < DepthWidth; x++)
                { 
                    int index = (y * DepthWidth) + x;
                    DepthImage2D[x, y] = depthImage[index];
                }
            }
            return DepthImage2D;
        }

        /// <summary>
        /// quick check to see if the pixels are within the screen calibration rect
        /// </summary>
        /// <param name="p">point to check</param>
        /// <returns>true if the point is within the calibration rect, false otherwise</returns>
        public bool IsWithinScreenCalibration(Point p)
        {
            if (p.X < boundaryFilterRect.Left)
                return false;
            if (p.X > boundaryFilterRect.Right)
                return false;

            if (p.Y < boundaryFilterRect.Top)
                return false;

            if (p.Y > boundaryFilterRect.Bottom)
                return false;

            return true;
        }

        /// <summary>
        /// this gets a simple threshold image of the current depth image. It can be used to seperate the 'depth layer' from the 'hover' layer
        /// </summary>
        /// <param name="depthImage">array of depth points</param>
        /// <param name="cutoff">where the depth cutoff is to stop analyzing</param>
        /// <param name="tolerance">tolerance off of the cuttoff point</param>
        /// <param name="pixelCount">number of pixels found. can be used to make later algos faster</param>
        /// <returns></returns>
        public int[] GetThresholdImage(int[] depthImage, int cutoff, int tolerance,out int pixelCount)
        {
            pixelCount = 0;
            ThresholdImage = new int[DepthWidth*DepthHeight];
            for (int i = 0; i < depthImage.Length; i++)
            {
                int pixelValue = 0;
                while ((i < depthImage.Length) && (depthImage[i] > 0))
                {
                    Point p = GetPointFromIndex(i, DepthWidth);
                    // Check against the calibration file where the 'screen dimensions' are set, and see
                    // if we're working with a pixel within those dimensions
                    if(!IsWithinScreenCalibration(p))
                    {
                        i++;
                        continue;
                    }

                    pixelValue = 0;

                    if (depthImage[i] > cutoff && depthImage[i] < tolerance)
                    { 
                        pixelValue++;

                        pixelCount++;
                                                
                        // Check up/down for neighboars. If 'active' then raise the 'score'.
                        int upIndex = (int)((p.Y - 1) * DepthWidth + p.X);
                        int downIndex = (int)((p.Y + 1) * DepthWidth + p.X);

                        if ((upIndex > 0) && (upIndex < depthImage.Length))
                        {
                            if (depthImage[upIndex] > 0)
                            {
                                pixelValue++;
                                ThresholdImage[upIndex] += pixelValue;
                            }

                            // If before pixel is 'active', then increase it's value.
                            if (depthImage[i-1] > 0)
                            {
                                ThresholdImage[i-1] = pixelValue;
                            }
                            ThresholdImage[i] += pixelValue;
                        }
                        if ((downIndex > 0) && (downIndex < depthImage.Length))
                        {
                            if (depthImage[downIndex] > 0)
                            {
                                pixelValue++;
                                ThresholdImage[downIndex] += pixelValue;
                            }
                        }
                    }

                    i++;
                    if (i == depthImage.Length)
                        break;
                }
            }
            return ThresholdImage;
        }
    }
}
